use std::borrow::Cow;

use dodgy_2d::AvoidanceOptions;
use reusable_counter::ReusableCounter;
use rstar::{PointDistance, RTree, AABB};
use rustc_hash::FxHashMap;
#[cfg(feature = "wasm")]
use wasm_bindgen::prelude::wasm_bindgen;

pub mod types;

use types::*;
#[cfg(feature = "wasm")]
use web_sys::js_sys::ArrayBuffer;

use crate::{mash_data::NapiNavMeshData, F64Point, NavMesh};

mod reusable_counter;

#[cfg(feature = "wasm")]
use web_sys::js_sys::Float64Array;

#[cfg(feature = "napi")]
use napi_derive::napi;

#[cfg_attr(feature = "wasm", wasm_bindgen)]
#[cfg_attr(feature = "napi", napi)]
pub struct AgentNavigationWorld {
    agents: FxHashMap<i32, (DodgyAgent, AgentParameters)>,
    obstacles: FxHashMap<i32, DodgyObstacle>,
    uid_counter: ReusableCounter,
    agent_r_tree: RTree<AgentAABBObject>,
    obstacle_r_tree: RTree<ObstacleAABBObject>,
    enable_agent_obstacle: bool, //是否启用agent的障碍物检测
    navmesh: NavMesh,
    edge_r_tree: RTree<EdgeAABBObject>,
}

#[cfg_attr(feature = "wasm", wasm_bindgen)]
#[cfg_attr(feature = "napi", napi)]
#[derive(Clone, Copy)]
pub struct AgentPosition {
    pub id: i32,
    pub pv: AgentPositionVelocity,
}

#[cfg_attr(feature = "wasm", wasm_bindgen)]
#[cfg_attr(feature = "napi", napi(object))]
#[derive(Clone, Copy)]
pub struct AgentPositionVelocity {
    pub position: F64Point,
    pub velocity: F64Point,
}

#[cfg(feature = "wasm")]
#[wasm_bindgen]
impl AgentNavigationWorld {
    #[wasm_bindgen(constructor)]
    pub fn new(buffer: ArrayBuffer, enable_agent_obstacle: bool) -> Self {
        let str = arraybuffer2str(&buffer).expect("get map str error");
        let navmesh = NavMesh::from_json(&str).unwrap();
        Self::with_navmesh(navmesh, enable_agent_obstacle)
    }

    /// 添加一个agent到场景中，此时不会直接将其放入RTree中，要等到调用rebuild_agent_r_tree才会
    pub fn add_agent(
        &mut self,
        agent_position_x: f64,
        agent_position_y: f64,
        agent_velocity_x: f64,
        agent_velocity_y: f64,
        agent_radius: f32,
        agent_avoidance_responsibility: f32,
        params_max_speed: f32,
        params_obstacle_margin_type: String,
        params_obstacle_margin_distance: Option<f64>,
        params_time_horizon: f32,
        params_obstacle_time_horizon: f32,
        params_check_agent_obstract: bool,
        params_has_obstract: bool,
        params_can_move: bool,
        params_target_point_x: Option<f64>,
        params_target_point_y: Option<f64>,
        params_min_check_stop_speed: f32,
        params_search_path_skip_distance: f64,
        params_entity_id: Option<String>,
        params_camp: Option<f64>,
        params_team_id: Option<f64>,
        params_entity_type_mask: Option<i32>,

    ) -> Option<i32> {
        let agent = Agent {
            position: F64Point::new(agent_position_x, agent_position_y),
            velocity: F64Point::new(agent_velocity_x, agent_velocity_y),
            radius: agent_radius,
            avoidance_responsibility: agent_avoidance_responsibility,
        };
        let params_obstacle_margin = SimulatorMargin {
            simulator_margint_type: match params_obstacle_margin_type.to_uppercase().as_str() {
                "DISTANCE" => SimulatorMarginType::Distance,
                "AGENTRADIUS" => SimulatorMarginType::AgentRadius,
                _ => SimulatorMarginType::AgentRadius,
            },
            distance: params_obstacle_margin_distance,
        };
        let params_target_point = if let (Some(x), Some(y)) = (params_target_point_x, params_target_point_y) {
            Some(F64Point::new(x, y))
        } else {
            None
        };
        let params = AgentParameters {
            max_speed: params_max_speed,
            obstacle_margin: params_obstacle_margin,
            time_horizon: params_time_horizon,
            obstacle_time_horizon: params_obstacle_time_horizon,
            check_agent_obstract: params_check_agent_obstract,
            has_obstract: params_has_obstract,
            can_move: params_can_move,
            target_point: params_target_point,
            min_check_stop_speed: params_min_check_stop_speed,
            search_path_skip_distance: params_search_path_skip_distance,
            entity_id: params_entity_id,
            camp: params_camp,
            team_id: params_team_id,
            entity_type_mask: params_entity_type_mask,
        };
        Some(self.insert_agent(agent, params))
    }

    pub fn add_obstacle(&mut self, polygon_x:&Float64Array, polygon_y:&Float64Array, check_direction:bool) -> Option<i32> {
        let polygon = polygon_x.to_vec().iter().zip(polygon_y.to_vec().iter()).map(|(x, y)| F64Point::new(x.clone(), y.clone())).collect();
        let obstacle = Obstacle {
            polygon,
            check_direction,
        };
        Some(self.insert_obstacle(obstacle))
    }

    pub fn get_agents_in_aabb(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        type_mask: Option<i32>,
    ) -> Vec<i32> {
        self._get_agents_in_aabb(min_x, min_y, max_x, max_y, type_mask)
    }

    pub fn get_entities_in_aabb(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        type_mask: Option<i32>,
    ) -> Vec<String> {
        self._get_entities_in_aabb(min_x, min_y, max_x, max_y, type_mask)
    }

    pub fn get_entities_in_aabb_with_camp(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        camp: f64,
        type_mask: Option<i32>,
    ) -> Vec<String> {
        self._get_entities_in_aabb_with_camp(min_x, min_y, max_x, max_y, camp, type_mask)
    }

    pub fn get_entities_in_aabb_without_camp(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        camp: f64,
        type_mask: Option<i32>,
    ) -> Vec<String> {
        self._get_entities_in_aabb_without_camp(min_x, min_y, max_x, max_y, camp, type_mask)
    }

    pub fn get_team_ids_in_aabb(&self, min_x: f64, min_y: f64, max_x: f64, max_y: f64) -> Vec<f64> {
        self._get_team_ids_in_aabb(min_x, min_y, max_x, max_y)
    }

    pub fn get_team_ids_in_aabb_with_camp(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        camp: f64,
    ) -> Vec<f64> {
        self._get_team_ids_in_aabb_with_camp(min_x, min_y, max_x, max_y, camp)
    }

    pub fn get_team_ids_in_aabb_without_camp(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        camp: f64,
    ) -> Vec<f64> {
        self._get_team_ids_in_aabb_without_camp(min_x, min_y, max_x, max_y, camp)
    }

    pub fn get_obstacles_in_aabb(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
    ) -> Vec<i32> {
        self._get_obstacles_in_aabb(min_x, min_y, max_x, max_y)
    }

    pub fn get_polygon_index_at_point(&self, x: f64, y: f64) -> Option<i32> {
        self._get_polygon_index_at_point(x, y)
    }

    pub fn set_agent_can_move(&mut self, uid: i32, can_move: bool) {
        self._set_agent_can_move(uid, can_move)
    }

    pub fn search_path_smooth(
        &self,
        start_x: f64,
        start_y: f64,
        end_x: f64,
        end_y: f64,
    ) -> Option<Vec<F64Point>> {
        self._search_path_smooth(start_x, start_y, end_x, end_y)
    }

    pub fn search_path(
        &self,
        start_x: f64,
        start_y: f64,
        end_x: f64,
        end_y: f64,
    ) -> Option<Vec<F64Point>> {
        self._search_path(start_x, start_y, end_x, end_y)
    }

    pub fn set_agent_enable_obstacle(&mut self, agent_index: i32, enable: bool) {
        self._set_agent_enable_obstacle(agent_index, enable)
    }

    pub fn set_max_speed(&mut self, uid: i32, max_speed: f32) {
        self._set_max_speed(uid, max_speed)
    }

    pub fn is_point_in_walkable_polygon(&self, point_x: f64, point_y: f64) -> bool {
        self._is_point_in_walkable_polygon(point_x, point_y)
    }

    pub fn move_agent(&mut self, uid: i32, target_x: f64, target_y: f64, search_path: bool, radius: Option<f64>, corner_points_count: Option<usize>) {
        let target = F64Point::new(target_x, target_y);
        self.move_agent_to(uid, Some(target), search_path, radius, corner_points_count);
    }

    pub fn stop_move(&mut self, uid: i32) {
        self._stop_move(uid)
    }

    pub fn get_agent_pos(&self, uid: i32) -> Option<AgentPositionVelocity> {
        self._get_agent_pos(uid)
    }

    pub fn get_agents_pos(&self, uids: Vec<i32>) -> Vec<AgentPosition> {
        self._get_agents_pos(uids)
    }

    pub fn get_all_agents_pos(&self) -> Vec<AgentPosition> {
        self._get_all_agents_pos()
    }

    pub fn get_agent_velocity(&self, uid: i32) -> F64Point {
        self._get_agent_velocity(uid)
    }

    pub fn remove_agent(&mut self, uid: i32) {
        self._remove_agent(uid)
    }

    pub fn remove_obstacle(&mut self, uid: i32) {
        self._remove_obstacle(uid)
    }

    pub fn search_nearest_valid_point_with_move_dir(
        &self,
        point_x: f64,
        point_y: f64,
        move_dir_x: f64,
        move_dir_y: f64,
    ) -> Option<F64Point> {
        self._search_nearest_valid_point_with_move_dir(point_x, point_y, move_dir_x, move_dir_y)
    }

    pub fn is_point_in_obstacle(&self, point_x: f64, point_y: f64) -> bool {
        self._is_point_in_obstacle(point_x, point_y)
    }

    pub fn teleport_agent(&mut self, uid: i32, point_x: f64, point_y: f64) {
        self._teleport_agent(uid, point_x, point_y)
    }

    pub fn enable_ignore_agent_obstacle(&mut self, uid: i32, enable: bool) {
        self._enable_ignore_agent_obstacle(uid, enable)
    }

    pub fn update(&mut self, time_step: f32) {
        self._update(time_step)
    }

    pub fn rebuild_agent_r_tree(&mut self) {
        self._rebuild_agent_r_tree()
    }

    pub fn set_agent_has_obstract(&mut self, uid: i32, has_obstract: bool) {
        self._set_agent_has_obstract(uid, has_obstract)
    }

    pub fn get_path_with_radius(&self, start_x: f64, start_y: f64, end_x: f64, end_y: f64, radius: f64, corner_points_count: usize) -> Option<Vec<F64Point>> {
        self._get_path_with_radius(start_x, start_y, end_x, end_y, radius, corner_points_count)
    }
}

#[cfg(feature = "wasm")]
fn arraybuffer2str(buffer: &web_sys::js_sys::ArrayBuffer) -> Option<String> {
    let uint8_array = web_sys::js_sys::Uint8Array::new(buffer);
    let vec = uint8_array.to_vec();
    String::from_utf8(vec).ok()
}

#[cfg(feature = "napi")]
#[napi]
impl AgentNavigationWorld {
    #[napi(constructor)]
    pub fn new(data:NapiNavMeshData, enable_agent_obstacle: bool) -> Self {
        let navmesh = NavMesh::from_napi_data(data);
        Self::with_navmesh(navmesh, enable_agent_obstacle)
    }

    #[napi]
    pub fn add_agent(&mut self, agent:NapiAgent, params:NapiAgentParameters) -> Option<i32> {
        Some(self.insert_agent(agent.into(), params.into()))
    }

    #[napi]
    pub fn add_obstacle(&mut self, obstacle: Obstacle) -> Option<i32> {
        Some(self.insert_obstacle(obstacle))
    }

    #[napi]
    pub fn get_agents_in_aabb(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        type_mask: Option<i32>,
    ) -> Vec<i32> {
        self._get_agents_in_aabb(min_x, min_y, max_x, max_y, type_mask)
    }

    #[napi]
    pub fn get_entities_in_aabb(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        type_mask: Option<i32>,
    ) -> Vec<String> {
        self._get_entities_in_aabb(min_x, min_y, max_x, max_y, type_mask)
    }

    #[napi]
    pub fn get_entities_in_aabb_with_camp(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        camp: f64,
        type_mask: Option<i32>,
    ) -> Vec<String> {
        self._get_entities_in_aabb_with_camp(min_x, min_y, max_x, max_y, camp, type_mask)
    }

    #[napi]
    pub fn get_entities_in_aabb_without_camp(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        camp: f64,
        type_mask: Option<i32>,
    ) -> Vec<String> {
        self._get_entities_in_aabb_without_camp(min_x, min_y, max_x, max_y, camp, type_mask)
    }

    #[napi]
    pub fn get_team_ids_in_aabb(&self, min_x: f64, min_y: f64, max_x: f64, max_y: f64) -> Vec<f64> {
        self._get_team_ids_in_aabb(min_x, min_y, max_x, max_y)
    }

    #[napi]
    pub fn get_team_ids_in_aabb_with_camp(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        camp: f64,
    ) -> Vec<f64> {
        self._get_team_ids_in_aabb_with_camp(min_x, min_y, max_x, max_y, camp)
    }

    #[napi]
    pub fn get_team_ids_in_aabb_without_camp(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        camp: f64,
    ) -> Vec<f64> {
        self._get_team_ids_in_aabb_without_camp(min_x, min_y, max_x, max_y, camp)
    }

    #[napi]
    pub fn get_obstacles_in_aabb(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
    ) -> Vec<i32> {
        self._get_obstacles_in_aabb(min_x, min_y, max_x, max_y)
    }

    #[napi]
    pub fn get_polygon_index_at_point(&self, x: f64, y: f64) -> Option<i32> {
        self._get_polygon_index_at_point(x, y)
    }

    #[napi]
    pub fn set_agent_can_move(&mut self, uid: i32, can_move: bool) {
        self._set_agent_can_move(uid, can_move)
    }

    #[napi]
    pub fn search_path_smooth(
        &self,
        start_x: f64,
        start_y: f64,
        end_x: f64,
        end_y: f64,
    ) -> Option<Vec<F64Point>> {
        self._search_path_smooth(start_x, start_y, end_x, end_y)
    }

    #[napi]
    pub fn search_path(
        &self,
        start_x: f64,
        start_y: f64,
        end_x: f64,
        end_y: f64,
    ) -> Option<Vec<F64Point>> {
        self._search_path(start_x, start_y, end_x, end_y)
    }

    #[napi]
    pub fn set_agent_enable_obstacle(&mut self, agent_index: i32, enable: bool) {
        self._set_agent_enable_obstacle(agent_index, enable)
    }

    #[napi]
    pub fn set_max_speed(&mut self, uid: i32, max_speed: f64) {
        self._set_max_speed(uid, max_speed as f32)
    }

    #[napi]
    pub fn is_point_in_walkable_polygon(&self, point_x: f64, point_y: f64) -> bool {
        self._is_point_in_walkable_polygon(point_x, point_y)
    }

    #[napi]
    pub fn move_agent(&mut self, uid: i32, target_x: f64, target_y: f64, search_path: bool, radius: Option<f64>, corner_points_count: Option<i64>) {
        let target = F64Point::new(target_x, target_y);
        let corner_points_count = corner_points_count.map(|v| v as usize);
        self.move_agent_to(uid, Some(target), search_path, radius, corner_points_count);
    }

    #[napi]
    pub fn stop_move(&mut self, uid: i32) {
        self._stop_move(uid)
    }

    #[napi]
    pub fn get_agent_pos(&self, uid: i32) -> Option<AgentPositionVelocity> {
        self._get_agent_pos(uid)
    }

    #[napi]
    pub fn get_agents_pos(&self, uids: Vec<i32>) -> Vec<AgentPosition> {
        self._get_agents_pos(uids)
    }

    #[napi]
    pub fn get_all_agents_pos(&self) -> Vec<AgentPosition> {
        self._get_all_agents_pos()
    }

    #[napi]
    pub fn get_agent_velocity(&self, uid: i32) -> F64Point {
        self._get_agent_velocity(uid)
    }

    #[napi]
    pub fn remove_agent(&mut self, uid: i32) {
        self._remove_agent(uid)
    }

    #[napi]
    pub fn remove_obstacle(&mut self, uid: i32) {
        self._remove_obstacle(uid)
    }

    #[napi]
    pub fn search_nearest_valid_point_with_move_dir(
        &self,
        point_x: f64,
        point_y: f64,
        move_dir_x: f64,
        move_dir_y: f64,
    ) -> Option<F64Point> {
        self._search_nearest_valid_point_with_move_dir(point_x, point_y, move_dir_x, move_dir_y)
    }

    #[napi]
    pub fn is_point_in_obstacle(&self, point_x: f64, point_y: f64) -> bool {
        self._is_point_in_obstacle(point_x, point_y)
    }

    #[napi]
    pub fn teleport_agent(&mut self, uid: i32, point_x: f64, point_y: f64) {
        self._teleport_agent(uid, point_x, point_y)
    }

    #[napi]
    pub fn enable_ignore_agent_obstacle(&mut self, uid: i32, enable: bool) {
        self._enable_ignore_agent_obstacle(uid, enable)
    }

    #[napi]
    pub fn update(&mut self, time_step: f64) {
        self._update(time_step as f32)
    }

    #[napi]
    pub fn rebuild_agent_r_tree(&mut self) {
        self._rebuild_agent_r_tree()
    }

    #[napi]
    pub fn set_agent_has_obstract(&mut self, uid: i32, has_obstract: bool) {
        self._set_agent_has_obstract(uid, has_obstract)
    }

    #[napi]
    pub fn get_path_with_radius(&self, start_x: f64, start_y: f64, end_x: f64, end_y: f64, radius: f64, corner_points_count: i64) -> Option<Vec<F64Point>> {
        let corner_points_count = corner_points_count as usize;
        self._get_path_with_radius(start_x, start_y, end_x, end_y, radius, corner_points_count)
    }
}

impl AgentNavigationWorld {
    pub fn _get_agents_in_aabb(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        type_mask: Option<i32>,
    ) -> Vec<i32> {
        let min = F64Point::new(min_x, min_y);
        let max = F64Point::new(max_x, max_y);
        let aabb = AABB::from_corners(min, max);
        self.search_agents_in_aabb(aabb, type_mask)
    }

    pub fn _get_entities_in_aabb(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        type_mask: Option<i32>,
    ) -> Vec<String> {
        let min = F64Point::new(min_x, min_y);
        let max = F64Point::new(max_x, max_y);
        let aabb = AABB::from_corners(min, max);
        self.search_entities_in_aabb(aabb, type_mask)
    }

    pub fn _get_entities_in_aabb_with_camp(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        camp: f64,
        type_mask: Option<i32>,
    ) -> Vec<String> {
        let min = F64Point::new(min_x, min_y);
        let max = F64Point::new(max_x, max_y);
        let aabb = AABB::from_corners(min, max);
        self.search_entities_in_aabb_with_camp(aabb, camp, type_mask)
    }

    pub fn _get_entities_in_aabb_without_camp(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        camp: f64,
        type_mask: Option<i32>,
    ) -> Vec<String> {
        let min = F64Point::new(min_x, min_y);
        let max = F64Point::new(max_x, max_y);
        let aabb = AABB::from_corners(min, max);
        self.search_entities_in_aabb_without_camp(aabb, camp, type_mask)
    }

    pub fn _get_team_ids_in_aabb(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
    ) -> Vec<f64> {
        let min = F64Point::new(min_x, min_y);
        let max = F64Point::new(max_x, max_y);
        let aabb = AABB::from_corners(min, max);
        self.search_team_ids_in_aabb(aabb)
    }

    pub fn _get_team_ids_in_aabb_with_camp(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        camp: f64,
    ) -> Vec<f64> {
        let min = F64Point::new(min_x, min_y);
        let max = F64Point::new(max_x, max_y);
        let aabb = AABB::from_corners(min, max);
        self.search_team_ids_in_aabb_with_camp(aabb, camp)
    }

    pub fn _get_team_ids_in_aabb_without_camp(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
        camp: f64,
    ) -> Vec<f64> {
        let min = F64Point::new(min_x, min_y);
        let max = F64Point::new(max_x, max_y);
        let aabb = AABB::from_corners(min, max);
        self.search_team_ids_in_aabb_without_camp(aabb, camp)
    }

    pub fn _get_obstacles_in_aabb(
        &self,
        min_x: f64,
        min_y: f64,
        max_x: f64,
        max_y: f64,
    ) -> Vec<i32> {
        let min = F64Point::new(min_x, min_y);
        let max = F64Point::new(max_x, max_y);
        let aabb = AABB::from_corners(min, max);
        self.search_obstacles_in_aabb(aabb)
    }

    pub fn _get_polygon_index_at_point(&self, x: f64, y: f64) -> Option<i32> {
        self.navmesh.get_polygon_index_at_point(x, y)
    }

    pub fn _set_agent_can_move(&mut self, uid: i32, can_move: bool) {
        if let Some((_, params)) = self.agents.get_mut(&uid) {
            params.can_move = can_move;
        }
    }

    pub fn _set_agent_has_obstract(&mut self, uid: i32, has_obstract: bool) {
        if let Some((_, params)) = self.agents.get_mut(&uid) {
            params.has_obstract = has_obstract;
        }
    }

    pub fn _search_path_smooth(
        &self,
        start_x: f64,
        start_y: f64,
        end_x: f64,
        end_y: f64,
    ) -> Option<Vec<F64Point>> {
        let start = F64Point::new(start_x, start_y);
        let end = F64Point::new(end_x, end_y);
        self.navmesh.search_path_smooth(&start, &end)
    }

    pub fn _search_path(
        &self,
        start_x: f64,
        start_y: f64,
        end_x: f64,
        end_y: f64,
    ) -> Option<Vec<F64Point>> {
        let start = F64Point::new(start_x, start_y);
        let end = F64Point::new(end_x, end_y);
        self.navmesh.search_path(&start, &end)
    }

    pub fn _set_agent_enable_obstacle(&mut self, agent_index: i32, enable: bool) {
        if let Some((_, params)) = self.agents.get_mut(&agent_index) {
            params.check_agent_obstract = enable;
        }
    }

    pub fn _set_max_speed(&mut self, uid: i32, max_speed: f32) {
        if let Some((_, params)) = self.agents.get_mut(&uid) {
            params.max_speed = max_speed;
        }
    }

    pub fn _is_point_in_walkable_polygon(&self, point_x: f64, point_y: f64) -> bool {
        let point = F64Point::new(point_x, point_y);
        self.navmesh.get_polygon_at_point(&point).is_some()
    }

    pub fn _move_agent(&mut self, uid: i32, target_x: f64, target_y: f64, search_path: bool) {
        let target = F64Point::new(target_x, target_y);
        self.move_agent_to(uid, Some(target), search_path, None, None);
    }

    pub fn _stop_move(&mut self, uid: i32) {
        self.move_agent_to(uid, None, false, None, None);
    }

    pub fn _get_agent_pos(&self, uid: i32) -> Option<AgentPositionVelocity> {
        if let Some((agent, _)) = self.agents.get(&uid) {
            return Some(AgentPositionVelocity {
                position: F64Point::new(agent.0.position.x as f64, agent.0.position.y as f64),
                velocity: F64Point::new(agent.0.velocity.x as f64, agent.0.velocity.y as f64),
            });
        }
        None
    }

    pub fn _get_agents_pos(&self, uids: Vec<i32>) -> Vec<AgentPosition> {
        let mut result = Vec::new();
        for uid in uids {
            if let Some((agent, _)) = self.agents.get(&uid) {
                result.push(AgentPosition {
                    id: uid,
                    pv: AgentPositionVelocity {
                        position: F64Point::new(
                            agent.0.position.x as f64,
                            agent.0.position.y as f64,
                        ),
                        velocity: F64Point::new(
                            agent.0.velocity.x as f64,
                            agent.0.velocity.y as f64,
                        ),
                    },
                });
            }
        }
        result
    }

    pub fn _get_all_agents_pos(&self) -> Vec<AgentPosition> {
        let mut result = Vec::new();
        for (uid, (agent, _)) in self.agents.iter() {
            result.push(AgentPosition {
                id: *uid,
                pv: AgentPositionVelocity {
                    position: F64Point::new(agent.0.position.x as f64, agent.0.position.y as f64),
                    velocity: F64Point::new(agent.0.velocity.x as f64, agent.0.velocity.y as f64),
                },
            });
        }
        result
    }

    pub fn _get_agent_velocity(&self, uid: i32) -> F64Point {
        if let Some((agent, _)) = self.agents.get(&uid) {
            return F64Point::new(agent.0.velocity.x as f64, agent.0.velocity.y as f64);
        }
        F64Point::new(0.0, 0.0)
    }

    pub fn _remove_agent(&mut self, uid: i32) {
        let Some(info) = self.agents.remove(&uid) else {
            return;
        };
        let aabb = info.0.get_aabb();
        self.agent_r_tree.remove(&AgentAABBObject {
            agent: uid,
            aabb: aabb,
            entity_id: None,
            camp: None,
            team_id: None,
            entity_type_mask: None,
        });
        self.uid_counter.recycle(uid);
    }

    pub fn _remove_obstacle(&mut self, uid: i32) {
        let Some(info) = self.obstacles.remove(&uid) else {
            return;
        };
        self.obstacle_r_tree.remove(&ObstacleAABBObject {
            obstacle: uid,
            aabb: info.get_aabb(),
        });
        self.uid_counter.recycle(uid);
    }

    pub fn _search_nearest_valid_point_with_move_dir(
        &self,
        point_x: f64,
        point_y: f64,
        move_dir_x: f64,
        move_dir_y: f64,
    ) -> Option<F64Point> {
        let point = F64Point::new(point_x, point_y);
        let move_dir = F64Point::new(move_dir_x, move_dir_y);
        self.navmesh
            .native_nearest_valid_point_with_move_dir(&point, &move_dir)
    }

    pub fn _is_point_in_obstacle(&self, point_x: f64, point_y: f64) -> bool {
        let point = F64Point::new(point_x, point_y);
        self.native_is_point_in_obstacle(&point)
    }

    pub fn _teleport_agent(&mut self, uid: i32, point_x: f64, point_y: f64) {
        let point = F64Point::new(point_x, point_y);
        self.set_agent_pos(uid, point);
    }

    pub fn _enable_ignore_agent_obstacle(&mut self, uid: i32, enable: bool) {
        if let Some((_, params)) = self.agents.get_mut(&uid) {
            params.check_agent_obstract = !enable;
        }
    }

    pub fn _update(&mut self, time_step: f32) {
        let mut new_velocities = Vec::with_capacity(self.agents.len());
        for (&index, (agent, params)) in self.agents.iter() {
            if !params.can_move {
                new_velocities.push(dodgy_2d::Vec2::new(0.0, 0.0));
                continue;
            }

            let target_point = if let Some(point) = params.target_point {
                point
            } else {
                F64Point::new(agent.0.position.x as f64, agent.0.position.y as f64)
            };

            //获取一条路径的AABB，这条路径经过的对象应该都需要检查
            let mut neighbor_agents = Vec::new();

            if self.enable_agent_obstacle && params.check_agent_obstract {
                let check_distance = params.max_speed * params.time_horizon + agent.0.radius * 2.0;
                let min_point =
                    agent.0.position - dodgy_2d::Vec2::new(check_distance, check_distance);
                let max_point =
                    agent.0.position + dodgy_2d::Vec2::new(check_distance, check_distance);
                let search_aabb = AABB::from_corners(
                    F64Point::new(min_point.x as f64, min_point.y as f64),
                    F64Point::new(max_point.x as f64, max_point.y as f64),
                );
                let neighors = self
                    .agent_r_tree
                    .locate_in_envelope_intersecting(&search_aabb);

                for aabb in neighors {
                    if aabb.agent == index {
                        continue;
                    }
                    let Some(agent) = self.agents.get(&aabb.agent) else {
                        continue;
                    };
                    if agent.1.has_obstract == false {
                        continue;
                    }
                    neighbor_agents.push(Cow::Borrowed(&agent.0 .0));
                }
            }

            let onstract_search_aabb = AABB::from_corners(
                F64Point::new(
                    (agent.0.position.x - params.max_speed) as f64,
                    (agent.0.position.y - params.max_speed) as f64,
                ),
                F64Point::new(
                    (agent.0.position.x + params.max_speed) as f64,
                    (agent.0.position.y + params.max_speed) as f64,
                ),
            );

            let mut neighbor_obstacles = Vec::new();
            let neighors = self
                .obstacle_r_tree
                .locate_in_envelope_intersecting(&onstract_search_aabb);
            for aabb in neighors {
                let index = aabb.obstacle;
                let Some(obstacle) = self.obstacles.get(&index) else {
                    continue;
                };
                neighbor_obstacles.push(Cow::Borrowed(&obstacle.obstacle));
            }

            let neighors = self
                .edge_r_tree
                .locate_in_envelope_intersecting(&onstract_search_aabb);
            for aabb in neighors {
                neighbor_obstacles.push(Cow::Borrowed(aabb.get_obstacle()));
            }

            let dir = dodgy_2d::Vec2::new(
                target_point.x as f32 - agent.0.position.x,
                target_point.y as f32 - agent.0.position.y,
            );

            let len = dir.length();
            let velocity = if len > params.max_speed * time_step {
                dir.normalize_or_zero() * params.max_speed
            } else {
                dir * (1.0 / time_step)
            };

            let obstacle_margin = match params.obstacle_margin.simulator_margint_type {
                SimulatorMarginType::AgentRadius => agent.0.radius,
                SimulatorMarginType::Distance => {
                    params.obstacle_margin.distance.unwrap_or(0.0) as f32
                }
            };

            let new_velocity = agent.0.compute_avoiding_velocity(
                &neighbor_agents,
                &neighbor_obstacles,
                velocity,
                params.max_speed,
                time_step,
                &AvoidanceOptions {
                    obstacle_margin: obstacle_margin,
                    time_horizon: f32_max(
                        f32_max(params.time_horizon, time_step + 1e-3),
                        obstacle_margin / params.max_speed * 1.1,
                    ),
                    obstacle_time_horizon: f32_max(
                        f32_max(params.obstacle_time_horizon, time_step + 1e-3),
                        obstacle_margin / params.max_speed * 1.1,
                    ),
                },
            );

            new_velocities.push(new_velocity);
        }

        for ((_, (agent, params)), new_velocity) in self.agents.iter_mut().zip(new_velocities) {
            let min_check_stop_speed = params.min_check_stop_speed;
            if new_velocity.length_squared() < min_check_stop_speed * min_check_stop_speed {
                agent.0.velocity = dodgy_2d::Vec2::ZERO;
                continue;
            }
            agent.0.position += new_velocity * time_step;
            agent.0.velocity = new_velocity;
        }
    }

    /**
     * 重建agent的rtree
     * 考虑到agent几乎每帧都会移动，更新每个节点的代价太大，且树容易不平衡，不如直接重建
     */
    pub fn _rebuild_agent_r_tree(&mut self) {
        self.agent_r_tree = RTree::bulk_load(
            self.agents
                .iter()
                .map(|(&uid, (agent, params))| AgentAABBObject {
                    agent: uid,
                    aabb: agent.get_aabb(),
                    entity_id: params.entity_id.clone(),
                    camp: params.camp,
                    team_id: params.team_id,
                    entity_type_mask: params.entity_type_mask,
                })
                .collect(),
        );
    }
}

fn f32_max(a: f32, b: f32) -> f32 {
    if a > b {
        a
    } else {
        b
    }
}

impl AgentNavigationWorld {
    pub fn with_js_string(str: &str, enable_agent_obstacle: bool) -> Self {
        Self::with_navmesh(NavMesh::from_json(str).unwrap(), enable_agent_obstacle)
    }

    pub fn with_navmesh(navmesh: NavMesh, enable_agent_obstacle: bool) -> Self {
        let edge_r_tree = RTree::bulk_load(
            navmesh
                .get_walkable_polygon_edges()
                .iter()
                .map(|edge| EdgeAABBObject::with_edge(edge))
                .collect(),
        );
        Self {
            agents: FxHashMap::default(),
            obstacles: FxHashMap::default(),
            uid_counter: ReusableCounter::new(),
            enable_agent_obstacle,
            agent_r_tree: RTree::new(),
            obstacle_r_tree: RTree::new(),
            navmesh,
            edge_r_tree,
        }
    }

    pub fn native_is_point_in_obstacle(&self, point: &F64Point) -> bool {
        let aabb = AABB::from_point(point.to_owned());
        let aabbs = self.obstacle_r_tree.locate_in_envelope_intersecting(&aabb);
        for aabb in aabbs {
            let Some(obstacle) = self.obstacles.get(&aabb.obstacle) else {
                continue;
            };
            if obstacle
                .is_point_in_closed_obstacle(dodgy_2d::Vec2::new(point.x as f32, point.y as f32))
            {
                return true;
            }
        }

        false
    }

    pub fn move_agent_to(&mut self, uid: i32, point: Option<F64Point>, search_path: bool, radius: Option<f64>, corner_points_count: Option<usize>) {
        if let Some((agent, param)) = self.agents.get_mut(&uid) {
            if let Some(target_point) = point {
                if search_path {
                    let start_point =
                        F64Point::new(agent.0.position.x as f64, agent.0.position.y as f64);
                    
                    let path = if let (Some(radius), Some(corner_points_count)) = (radius, corner_points_count) {
                        self.navmesh.get_path_with_radius(
                            start_point.x,
                            start_point.y,
                            target_point.x,
                            target_point.y,
                            radius,
                            corner_points_count
                        )
                    } else {
                        self.navmesh.search_path_smooth(&start_point, &target_point)
                    };

                    if let Some(path) = path {
                        // 寻找下一个目标点
                        match path.len() {
                            2 => {
                                param.target_point = Some(path[1]);
                            }
                            n if n > 2 => {
                                if path[1].distance_2(&start_point)
                                    < (param.search_path_skip_distance
                                        * param.search_path_skip_distance)
                                {
                                    param.target_point = Some(path[2]);
                                } else {
                                    param.target_point = Some(path[1]);
                                }
                            }
                            _ => {
                                param.target_point = Some(target_point);
                            }
                        }
                    } else {
                        param.target_point = Some(target_point);
                    }
                } else {
                    param.target_point = Some(target_point);
                }
            } else {
                param.target_point = None;
            }
        }
    }

    pub fn search_agents_in_aabb(&self, aabb: AABB<F64Point>, type_mask: Option<i32>) -> Vec<i32> {
        let mut result = Vec::new();
        let agents = self.agent_r_tree.locate_in_envelope_intersecting(&aabb);
        for agent in agents {
            // result.push(agent.agent);
            if let Some(entity_type_mask) = agent.entity_type_mask {
                if let Some(type_mask) = type_mask {
                    if entity_type_mask & type_mask != 0 {
                        result.push(agent.agent);
                    }
                } else {
                    result.push(agent.agent);
                }
            } else {
                result.push(agent.agent);
            }
        }
        result
    }

    pub fn search_entities_in_aabb(
        &self,
        aabb: AABB<F64Point>,
        type_mask: Option<i32>,
    ) -> Vec<String> {
        let mut result = Vec::new();
        let agents = self.agent_r_tree.locate_in_envelope_intersecting(&aabb);
        for agent in agents {
            if let Some(entity_id) = &agent.entity_id {
                if let Some(entity_type_mask) = agent.entity_type_mask {
                    if let Some(type_mask) = type_mask {
                        if entity_type_mask & type_mask != 0 {
                            result.push(entity_id.clone());
                        }
                    } else {
                        result.push(entity_id.clone());
                    }
                } else {
                    result.push(entity_id.clone());
                }
            }
        }
        result
    }

    pub fn search_entities_in_aabb_with_camp(
        &self,
        aabb: AABB<F64Point>,
        camp: f64,
        type_mask: Option<i32>,
    ) -> Vec<String> {
        let mut result = Vec::new();
        let agents = self.agent_r_tree.locate_in_envelope_intersecting(&aabb);
        for agent in agents {
            if let Some(entity_id) = &agent.entity_id {
                if let Some(agent_camp) = agent.camp {
                    if agent_camp == camp {
                        if let Some(entity_type_mask) = agent.entity_type_mask {
                            if let Some(type_mask) = type_mask {
                                if entity_type_mask & type_mask != 0 {
                                    result.push(entity_id.clone());
                                }
                            } else {
                                result.push(entity_id.clone());
                            }
                        } else {
                            result.push(entity_id.clone());
                        }
                    }
                }
            }
        }
        result
    }

    pub fn search_entities_in_aabb_without_camp(
        &self,
        aabb: AABB<F64Point>,
        camp: f64,
        type_mask: Option<i32>,
    ) -> Vec<String> {
        let mut result = Vec::new();
        let agents = self.agent_r_tree.locate_in_envelope_intersecting(&aabb);
        for agent in agents {
            if let Some(entity_id) = &agent.entity_id {
                if let Some(agent_camp) = agent.camp {
                    if agent_camp != camp {
                        if let Some(entity_type_mask) = agent.entity_type_mask {
                            if let Some(type_mask) = type_mask {
                                if entity_type_mask & type_mask != 0 {
                                    result.push(entity_id.clone());
                                }
                            } else {
                                result.push(entity_id.clone());
                            }
                        } else {
                            result.push(entity_id.clone());
                        }
                    }
                }
            }
        }
        result
    }

    pub fn search_team_ids_in_aabb(&self, aabb: AABB<F64Point>) -> Vec<f64> {
        let mut result = Vec::new();
        let agents = self.agent_r_tree.locate_in_envelope_intersecting(&aabb);
        for agent in agents {
            if let Some(team_id) = agent.team_id {
                result.push(team_id);
            }
        }
        result
    }

    pub fn search_team_ids_in_aabb_with_camp(&self, aabb: AABB<F64Point>, camp: f64) -> Vec<f64> {
        let mut result = Vec::new();
        let agents = self.agent_r_tree.locate_in_envelope_intersecting(&aabb);
        for agent in agents {
            if let Some(agent_camp) = agent.camp {
                if agent_camp == camp {
                    if let Some(team_id) = agent.team_id {
                        result.push(team_id);
                    }
                }
            }
        }
        result
    }

    pub fn search_team_ids_in_aabb_without_camp(
        &self,
        aabb: AABB<F64Point>,
        camp: f64,
    ) -> Vec<f64> {
        let mut result = Vec::new();
        let agents = self.agent_r_tree.locate_in_envelope_intersecting(&aabb);
        for agent in agents {
            if let Some(agent_camp) = agent.camp {
                if agent_camp != camp {
                    if let Some(team_id) = agent.team_id {
                        result.push(team_id);
                    }
                }
            }
        }
        result
    }

    pub fn search_obstacles_in_aabb(&self, aabb: AABB<F64Point>) -> Vec<i32> {
        let mut result = Vec::new();
        let obstacles = self.obstacle_r_tree.locate_in_envelope_intersecting(&aabb);
        for obstacle in obstacles {
            result.push(obstacle.obstacle);
        }
        result
    }

    pub fn insert_agent(&mut self, agent: Agent, params: AgentParameters) -> i32 {
        let uid = self.uid_counter.next();
        self.agents.insert(uid, (agent.into(), params));
        uid
    }

    pub fn insert_obstacle(&mut self, obstacle: Obstacle) -> i32 {
        let uid = self.uid_counter.next();
        let obstacle: DodgyObstacle = obstacle.into();
        let aabb = obstacle.get_aabb();
        self.obstacle_r_tree.insert(ObstacleAABBObject {
            obstacle: uid,
            aabb: aabb,
        });
        self.obstacles.insert(uid, obstacle);
        uid
    }

    pub fn set_agent_pos(&mut self, uid: i32, point: F64Point) {
        if let Some((agent, params)) = self.agents.get_mut(&uid) {
            agent.0.position = dodgy_2d::Vec2::new(point.x as f32, point.y as f32);
            agent.0.velocity = dodgy_2d::Vec2::ZERO;
            params.target_point = None;
        }
    }

    pub fn _get_path_with_radius(&self, start_x: f64, start_y: f64, end_x: f64, end_y: f64, radius: f64, corner_points_count: usize) -> Option<Vec<F64Point>> {
        let start_point = F64Point::new(start_x, start_y);
        let end_point = F64Point::new(end_x, end_y);
        let path = self.navmesh.get_path_with_radius(start_x, start_y, end_x, end_y, radius, corner_points_count);
        if let Some(path) = path {
            Some(path)
        } else {
            None
        }
    }
    
}
